#ifndef __AXIS_HPP
#define __AXIS_HPP

#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif

#include <array>

class Axis{
public:
    enum AxisState {
            AXIS_STATE_UNDEFINED             = 0,
            AXIS_STATE_IDLE                  = 1,
            AXIS_STATE_STARTUP_SEQUENCE      = 2,
            AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,
            AXIS_STATE_MOTOR_CALIBRATION     = 4,
            AXIS_STATE_SENSORLESS_CONTROL    = 5,
            AXIS_STATE_ENCODER_INDEX_SEARCH  = 6,
            AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7,
            AXIS_STATE_CLOSED_LOOP_CONTROL   = 8,
            AXIS_STATE_LOCKIN_SPIN           = 9,
            AXIS_STATE_ENCODER_DIR_FIND      = 10,
            AXIS_STATE_HOMING                = 11,
            AXIS_STATE_ZERO_TEST             = 12,
        };
        enum Error {
            ERROR_NONE                       = 0x00000000,
            ERROR_INVALID_STATE              = 0x00000001,
            ERROR_DC_BUS_UNDER_VOLTAGE       = 0x00000002,
            ERROR_DC_BUS_OVER_VOLTAGE        = 0x00000004,
            ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x00000008,
            ERROR_BRAKE_RESISTOR_DISARMED    = 0x00000010,
            ERROR_MOTOR_DISARMED             = 0x00000020,
            ERROR_MOTOR_FAILED               = 0x00000040,
            ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x00000080,
            ERROR_ENCODER_FAILED             = 0x00000100,
            ERROR_CONTROLLER_FAILED          = 0x00000200,
            ERROR_POS_CTRL_DURING_SENSORLESS = 0x00000400,
            ERROR_WATCHDOG_TIMER_EXPIRED     = 0x00000800,
            ERROR_MIN_ENDSTOP_PRESSED        = 0x00001000,
            ERROR_MAX_ENDSTOP_PRESSED        = 0x00002000,
            ERROR_ESTOP_REQUESTED            = 0x00004000,
            ERROR_CURRENT_MEASURE_FAULT      = 0x00008000,
            ERROR_HOMING_WITHOUT_ENDSTOP     = 0x00020000,
            ERROR_OVER_TEMP                  = 0x00040000,
            ERROR_STOP                       = 0x00080000,
        };

    struct Config_t {
        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
        bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
                                                // this only has an effect if encoder.config.use_index is also true
        bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
        bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
        bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
        bool startup_homing = false; //<! enable homing after calibration/startup

        bool enable_step_dir = false; //<! enable step/dir input after calibration
                                    //   For M0 this has no effect if enable_uart is true
        bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
                                         //<! This is ignored if enable_step_dir is false.
                                         //<! This setting only takes effect on a state transition
                                         //<! into idle or out of closed loop control.

        float turns_per_step = 1.0f / 1024.0f;

        float watchdog_timeout = 0.0f; // [s]
        bool enable_watchdog = false;

        // Defaults loaded from hw_config in load_configuration in main.cpp
        uint16_t step_gpio_pin = 1;
        uint16_t dir_gpio_pin = 2;

        uint32_t can_node_id = 1; // Both axes will have the same id to start
        bool can_node_id_extended = true;
		
        uint32_t can_heartbeat_rate_ms = 100;
		uint32_t can_encoder_count_rate_ms = 20;
		uint32_t can_updata_error_ms = 100;
		
		//APP
		uint16_t enable_Timestamp = 1;
		bool enable_debug = true;

        // custom setters
        Axis* parent = nullptr;
        void set_step_gpio_pin(uint16_t value) { step_gpio_pin = value; }
        void set_dir_gpio_pin(uint16_t value) { dir_gpio_pin = value;  }
		void load_set_can_node_id(uint16_t value) { can_node_id = value;}
		void load_set_can_times(uint16_t value) { enable_Timestamp = value;}
		
    };

    struct Homing_t {
        bool is_homed = false;
    };

    enum thread_signals {
        M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
    };

    Axis(int axis_num,
            const AxisHardwareConfig_t& hw_config,
            Config_t& config,
            Encoder& encoder,
            Controller& controller,
            Motor& motor);

    void setup();
    void start_thread();
    void signal_current_meas();
    bool wait_for_current_meas();

    void step_cb();

    bool check_DRV_fault();
    bool check_PSU_brownout();
    bool do_checks();
    bool do_updates();
	bool do_update_vel();
	bool check_axis_status();
	
	float Speed_Convert_estimate(int16_t val);  

    void watchdog_feed();
    bool watchdog_check();

    void clear_errors() {
        motor_.error_ = Motor::ERROR_NONE;
        controller_.error_ = Controller::ERROR_NONE;
		
        encoder_.spi_error_rate_ = 0.0f;
		encoder_.error_differ_times_ = 0;
		encoder_.count_in_cpr_ = encoder_.pos_abs_;
		
		controller_.vel_tolerance_times_ = 0;
		differ_state_ = 0;
		
		encoder_.error_ = Encoder::ERROR_NONE;

        error_ = ERROR_NONE;
    }

    // True if there are no errors
    bool inline check_for_errors() {
        if((error_ == ERROR_INVALID_STATE) || (error_ == ERROR_NONE)){
			return true;
		}
        return false;
    }

    // @brief Runs the specified update handler at the frequency of the current measurements.
    //
    // The loop runs until one of the following conditions:
    //  - update_handler returns false
    //  - the current measurement times out
    //  - the health checks fail (brownout, driver fault line)
    //  - update_handler doesn't update the modulation timings in time
    //    This criterion is ignored if current_state is AXIS_STATE_IDLE
    //
    // If update_handler is going to update the motor timings, you must call motor.arm()
    // shortly before this function.
    //
    // If the function returns, it is guaranteed that error is non-zero, except if the cause
    // for the exit was a negative return value of update_handler or an external
    // state change request (requested_state != AXIS_STATE_DONT_CARE).
    // Under all exit conditions the motor is disarmed and the brake current set to zero.
    // Furthermore, if the update_handler does not set the phase voltages in time, they will
    // go to zero.
    //
    // @tparam T Must be a callable type that takes no arguments and returns a bool
    template<typename T>
    void run_control_loop(const T& update_handler) {
        while (requested_state_ == AXIS_STATE_UNDEFINED) {
            // look for errors at axis level and also all subcomponents
            bool checks_ok = do_checks();
            // Update all estimators
            // Note: updates run even if checks fail
            bool updates_ok = do_updates(); 

            // make sure the watchdog is being fed. 
            bool watchdog_ok = watchdog_check();
            
            if (!checks_ok || !updates_ok || !watchdog_ok) {
                // It's not useful to quit idle since that is the safe action
                // Also leaving idle would rearm the motors
                if (current_state_ != AXIS_STATE_IDLE)
                    break;
            }

            // Run main loop function, defer quitting for after wait
            // TODO: change arming logic to arm after waiting
            bool main_continue = update_handler();

            // Check we meet deadlines after queueing
            ++loop_counter_;

            // Wait until the current measurement interrupt fires
            if (!wait_for_current_meas()) {
                // maybe the interrupt handler is dead, let's be
                // safe and float the phases
                safety_critical_disarm_motor_pwm(motor_);
                //update_brake_current();
                error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT;
                break;
            }

            if (!main_continue)
                break;
        }
    }

    bool run_closed_loop_control_loop();
    bool run_idle_loop();
	bool switch_encoder_mode();
    bool run_zero_test();

    constexpr uint32_t get_watchdog_reset() {
        //return static_cast<uint32_t>(std::clamp<float>(config_.watchdog_timeout, 0, UINT32_MAX / (current_meas_hz + 1)) * current_meas_hz);
    }

    void run_state_machine_loop();

    int axis_num_;
    const AxisHardwareConfig_t& hw_config_;
    Config_t& config_;
    
    Motor& motor_;
    Encoder& encoder_;
    Controller& controller_;

    osThreadId thread_id_;
    const uint32_t stack_size_ = 2048; // Bytes
    volatile bool thread_id_valid_ = false;

    // variables exposed on protocol
    Error error_ = ERROR_NONE;

    // updated from config in constructor, and on protocol hook

    AxisState requested_state_ = AXIS_STATE_UNDEFINED;
    std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
    AxisState& current_state_ = task_chain_.front();
    uint32_t loop_counter_ = 0;
    uint32_t last_heartbeat_ = 0;
	uint32_t last_encoder_count_ = 0;
	uint32_t last_updata_error_ = 0;
	uint32_t encoder_count_times_ = 1;  //1~180000

    uint8_t differ_state_ = 0;
	
    bool flag_axis_en = false;
	
    // watchdog
    uint32_t watchdog_current_value_= 0;
	float torque_setpoint;
/*
typedef union user_data_
{
    int32_t User_s32;
    uint32_t User_u32;
    int16_t User_s16[2];
    uint16_t User_u16[2];
    int8_t User_s8[4];
    uint8_t User_u8[4];
    float  User_float;
}user_data;
	user_data user_data1[6401];
    user_data user_data2[6401];
    //user_data user_data3[3200];
    //user_data user_data4[1600];
*/
    float DC_sum_phB = 0.0f;
    float DC_sum_phC = 0.0f;
    int32_t num = 0;
};


#endif /* __AXIS_HPP */
